/// \file otmm.pde
/// \name oTMM - open Thermal Mark Maker
/// \brief Software for apply Thermal Marks in capillary electrophoresis systems
/// \version 0.1-20080306
/// \author Coppyright 2008 - Carlos A. Neves <caneves@gmail.com>
/// \note Target MCU : Arduino/Freeduino
/// \note Compiler: Arduino Software version 0010
///
///   This program is free software; you can redistribute it and/or modify
///   it under the terms of the GNU General Public License as published by
///   the Free Software Foundation; either version 2, or (at your option)
///   any later version.
///
///   This program is distributed in the hope that it will be useful,
///   but WITHOUT ANY WARRANTY; without even the implied warranty of
///   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
///   GNU General Public License for more details.
///
///   You should have received a copy of the GNU General Public License
///   along with this program; if not, write to the Free Software Foundation,
///   Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.  
///

// includes
#include <EEPROM.h>

// defines
#define CALC_DUTY_CYCLE(x)		((unsigned char)((x*255)/100))

// Address parameters in the EEPROM memory
#define ADR_PULSE_WIDTH_TM			0	///< two bytes
#define ADR_PERCENT_MAX_POWER_TM1		2	///< one byte
#define ADR_PERCENT_MAX_POWER_TM2		3	///< one byte
#define ADR_INTERVAL_BETWEEN_CONTINUOUS_TM_MS	4 	///< two bytes
#define ADR_NUMBER_OF_TM			6	///< two bytes
#define ADR_INITIAL_DELAY_TM_MS			8	///< two bytes
#define ADR_TRIGGER_MODE                        10      ///< one byte

// pins
#define TM_TRIGGER_PIN         3  ///< Thermal Marker Start button (INPUT): active in high
#define TM_TRIGGER_RESET_PIN   4  ///< Thermal Marker Reset button(OUTPUT): active in high
#define TM_1_STATUS_PIN        5  ///< Status for burned TM1
#define TM_2_STATUS_PIN        6  ///< Status for burned TM2
#define PWM_1_PIN              9  ///< PWM A (OC1A)
#define PWM_2_PIN              10 ///< PWM B (OC2A)
#define TM_LED_PIN             13 ///< LED connected to digital pin 13

// default values
#define PULSE_WIDTH_TM_MS_DEFAULT                    100
#define PERCENT_MAX_POWER_TM1_DEFAULT                50
#define PERCENT_MAX_POWER_TM2_DEFAULT                0
#define INTERVAL_BETWEEN_CONTINUOUS_TM_MS_DEFAULT    0
#define NUMBER_OF_TM_DEFAULT                         1
#define INITIAL_DELAY_TM_MS_DEFAULT                  0
#define TRIGGER_MODE_DEFAULT                         1

// general
#define MY_RX_STRING_SIZE  11 		///< Size of serial read string

// general variables
char myRxData[MY_RX_STRING_SIZE]; 	///< string for I/O characteres
unsigned int i; 			///< ordinary 16-bit counter
unsigned char c; 			///< character
unsigned int counterTM; 		///< TM counter
char yn;				///< confirmation
char triggerModeOption;                 ///< trigger mode option

/// parameters variables
unsigned int pulseWidthTM_ms; 	                ///< pulse width of a thermal mark
char percentMaxPowerTM1; 			///< percent of max power
char percentMaxPowerTM2; 			///< percent of max power
unsigned int intervalBetweenContinuousTM_ms;    ///< max of 65535ms
unsigned int numberOfTM; 			///< max of 65535 makers
unsigned int initialDelayTM_ms;                 ///< max of 65535ms
unsigned char triggerMode;                      ///< s=by software, h=by hardware

/// functions prototype
void setup(void);
void loop(void);
unsigned char getTMStatusFunction(void);
char enterChar(void);
void optionsLoopFunction(void);
void helpFunction(void);
void setDefaultValuesFunction(void);
void setPulseWidthTMFunction(void);
void setPercentMaxPowerTM1Function(void);
void setPercentMaxPowerTM2Function(void);
void setIntervalBetweenContinousTMFunction(void);
void setNumberOfTMFunction(void);
void setInitialDelayTMFunction(void);
void setTriggerModeFunction(void);
void getParametersFunction(void);
unsigned char checkTriggerFunction(void);

void setup(void){

  Serial.begin(38400); 		  // opens serial port, sets data rate

  // configure digital lines
  pinMode(TM_TRIGGER_PIN, INPUT);         // sets the digital pin as input
  pinMode(TM_TRIGGER_RESET_PIN, OUTPUT);  // sets the digital pin as input
  pinMode(TM_1_STATUS_PIN, INPUT); 	  // sets the digital pin as input
  pinMode(TM_2_STATUS_PIN, INPUT); 	  // sets the digital pin as input
  pinMode(PWM_1_PIN, OUTPUT); 		  // sets the digital pin as output
  pinMode(PWM_2_PIN, OUTPUT); 		  // sets the digital pin as output  
  pinMode(TM_LED_PIN, OUTPUT); 		  // sets the digital pin as output

  // turn led off
  digitalWrite(TM_LED_PIN, LOW);		// turn led off

  // reset trigger
  digitalWrite(TM_TRIGGER_RESET_PIN, LOW);     // reset active in low
  delay(100);
  digitalWrite(TM_TRIGGER_RESET_PIN, HIGH);     // reset active in low

  // read parameters from EEPROM memory
  // set the pulse width
  pulseWidthTM_ms  = (unsigned int)(EEPROM.read(ADR_PULSE_WIDTH_TM+0)<<8); 	// Most Significant Byte (MSB)
  pulseWidthTM_ms |= (unsigned int)(EEPROM.read(ADR_PULSE_WIDTH_TM+1)<<0); 	// Least Significant Byte (LSB)

  // set the duty cycle of the channel A output
  percentMaxPowerTM1 = (unsigned char)EEPROM.read(ADR_PERCENT_MAX_POWER_TM1); 
  // set the duty cycle of the channel B output
  percentMaxPowerTM2 = (unsigned char)EEPROM.read(ADR_PERCENT_MAX_POWER_TM2); 

  // set the interval between TM in continuous mode
  intervalBetweenContinuousTM_ms  = (unsigned int)(EEPROM.read(ADR_INTERVAL_BETWEEN_CONTINUOUS_TM_MS+0)<<8); // Most Significant Byte (MSB)
  intervalBetweenContinuousTM_ms |= (unsigned int)(EEPROM.read(ADR_INTERVAL_BETWEEN_CONTINUOUS_TM_MS+1)<<0); // Least Significant Byte (LSB)

  // set the number of TM
  numberOfTM  = (unsigned int)(EEPROM.read(ADR_NUMBER_OF_TM+0)<<8); 		// Most Significant Byte (MSB)
  numberOfTM |= (unsigned int)(EEPROM.read(ADR_NUMBER_OF_TM+1)<<0); 		// Least Significant Byte (LSB)

  // set initial delay just after the start trigger
  initialDelayTM_ms  = (unsigned int)(EEPROM.read(ADR_INITIAL_DELAY_TM_MS+0)<<8); // Most Significant Byte (MSB)
  initialDelayTM_ms |= (unsigned int)(EEPROM.read(ADR_INITIAL_DELAY_TM_MS+1)<<0); // Least Significant Byte (LSB)

  // set the trigger mode
  triggerMode = (unsigned char)EEPROM.read(ADR_TRIGGER_MODE); 

  // process options
  optionsLoopFunction();

}
void loop(void){

  // run TM in loop applied by buttons
  //runLoopTMFunction();
}

/// Receive a string by serial and remove the CR/LF termination and
// add the \0 termination for atoi/atol function.
void enterString(void){

  // At MS-Windows environment, strings end with \r\n (CR LF) sequence.
  // This function does not works with the Arduino Serial Monitor.
  // Use another serial terminal like gtkterm (Linux), cutecom (Linux),
  // and hyperterminal (MS-Windows).

  char sc;   // serial character
  char flag; // ordinary flag

    Serial.flush();

  // Enter with the string
  i = 0;
  flag = 1;
  while(flag) {
    while((i < MY_RX_STRING_SIZE) && (Serial.available())){ // check for keypress
      sc = Serial.read();
      if (sc != '\r'){
        if (sc != '\n'){
          Serial.print(sc);
          myRxData[i] = sc;
          i++;
        }
        else{
          flag = 0;
        }
      }
    }
  }
  myRxData[i]='\0';     // end of string
  Serial.println();
  Serial.flush();       // clear any trash
}

/// Receive a character by serial port.
char enterChar(void){

  char ch = ' ';

  Serial.flush();
  // check for keypress
  while(Serial.available() == 0);
  ch = Serial.read();
  return ch;

}

/// Loop of options
void optionsLoopFunction(void){

  Serial.println();
  Serial.println("open Thermal Marker Maker v0.1");
  Serial.println("http://code.google.com/p/otmm/");

  // print new prompt
  Serial.println();
  Serial.print("cmd>");
  Serial.flush();

  // testing loop
  while(1){
    // reset the trigger
    digitalWrite(TM_TRIGGER_RESET_PIN, LOW);

    // check for keypress
    if(Serial.available() > 0){
      c = Serial.read();
      switch(c){
      case 'h':
        helpFunction();
        break;
      case 'd':
        setDefaultValuesFunction();
        break;
      case 'w':
        setPulseWidthTMFunction();
        break;
      case '1':
        setPercentMaxPowerTM1Function();
        break;
      case '2':
        setPercentMaxPowerTM2Function();
        break;
      case 'i':
        setIntervalBetweenContinousTMFunction();
        break;
      case 'n':
        setNumberOfTMFunction();
        break;
      case 'e':
        setInitialDelayTMFunction();
        break;
      case 't':
        setTriggerModeFunction();
        break;
      case 'p':
        getParametersFunction();
        break;
      case 'o':
        runLoopTMFunction();
        break;
      case 'm':
        getTMStatusFunction();
        break;
      case '\n':
        break;
      default:
        break;
      }
      // print new prompt
      Serial.println();
      Serial.print("cmd>");
    }
  }
}

/// Help functions
void helpFunction(void){

  Serial.println();

#ifdef __AVR_ATmega168__
  Serial.println("Commands");
  Serial.println("h - help");
  Serial.println();
  Serial.println("Configuration");
  Serial.println("d - set default values");
  Serial.println("w - set pulse width (0-65535ms)");
  Serial.println("1 - set percent of max power of TM1 (0-100%)");
  Serial.println("2 - set percent of max power of TM2 (0-100%)");
  Serial.println("i - set interval between TM (0-65535ms)");
  Serial.println("n - set number of TM (0=infinite or 1-65535)");
  Serial.println("e - set initial delay (0-65535ms)");
  Serial.println("t - set trigger mode (s=software, h=hardware)");
  Serial.println();
  Serial.println("Operation");
  Serial.println("o - enter in loop and wait the trigger signal");
  Serial.println("    Press trigger buttom to start/stop or");
  Serial.println("    external trigger to high to start and to");
  Serial.println("    low to stop, or send s by serial to start");
  Serial.println("    or q to quit"");
  Serial.println("s - start TM");
  Serial.println("q - quit TM");
  Serial.println();
  Serial.println("Status");
  Serial.println("p - print parameters");
  Serial.println("m - print the TM status");
  Serial.println();
#endif
#ifdef __AVR_ATmega8__
  Serial.println("oTMM Manual");
#endif
  Serial.println("http://code.google.com/p/otmm/");
}

/// Load the default values
void setDefaultValuesFunction(void){

  Serial.println();
  Serial.print("y/n: ");

  //check for keypress
  yn = enterChar();
  if(yn != 'y'){
    Serial.println("Abort");
    return;
  }

  // write parameters to EEPROM memory
  pulseWidthTM_ms = PULSE_WIDTH_TM_MS_DEFAULT;
  EEPROM.write(ADR_PULSE_WIDTH_TM+0, (unsigned char)((pulseWidthTM_ms>>8)&0xff));
  EEPROM.write(ADR_PULSE_WIDTH_TM+1, (unsigned char)((pulseWidthTM_ms>>0)&0xff));

  percentMaxPowerTM1 = PERCENT_MAX_POWER_TM1_DEFAULT;
  EEPROM.write(ADR_PERCENT_MAX_POWER_TM1, percentMaxPowerTM1);

  percentMaxPowerTM2 = PERCENT_MAX_POWER_TM2_DEFAULT;
  EEPROM.write(ADR_PERCENT_MAX_POWER_TM2, percentMaxPowerTM2);

  intervalBetweenContinuousTM_ms = INTERVAL_BETWEEN_CONTINUOUS_TM_MS_DEFAULT;
  EEPROM.write(ADR_INTERVAL_BETWEEN_CONTINUOUS_TM_MS+0, (unsigned char)((intervalBetweenContinuousTM_ms>>8)&0xff));
  EEPROM.write(ADR_INTERVAL_BETWEEN_CONTINUOUS_TM_MS+1, (unsigned char)((intervalBetweenContinuousTM_ms>>0)&0xff));

  numberOfTM = NUMBER_OF_TM_DEFAULT;
  EEPROM.write(ADR_NUMBER_OF_TM+0, (unsigned char)((numberOfTM>>8)&0xff));
  EEPROM.write(ADR_NUMBER_OF_TM+1, (unsigned char)((numberOfTM>>0)&0xff));

  initialDelayTM_ms = INITIAL_DELAY_TM_MS_DEFAULT;
  EEPROM.write(ADR_INITIAL_DELAY_TM_MS+0, (unsigned char)((initialDelayTM_ms>>8)&0xff));
  EEPROM.write(ADR_INITIAL_DELAY_TM_MS+1, (unsigned char)((initialDelayTM_ms>>0)&0xff));

  triggerMode = TRIGGER_MODE_DEFAULT;
  EEPROM.write(ADR_TRIGGER_MODE, triggerMode);
}

/// Set the pulse width value
void setPulseWidthTMFunction(void){

  Serial.println();
  Serial.print("Pulse width (0-65535 ms): ");

  // Enter with the value
  enterString();
  i = (unsigned int)atoi(myRxData);
  Serial.println();

  Serial.print("Value: ");
  Serial.print(i, DEC);
  Serial.println("ms");

  Serial.print("y/n: ");

  // check for keypress
  yn = enterChar();
  if(yn != 'y'){
    Serial.println("Abort");
    return;
  }

  pulseWidthTM_ms = i;

  // wrinting in the eeprom
  EEPROM.write(ADR_PULSE_WIDTH_TM+0, (unsigned char)((pulseWidthTM_ms>>8)&0xff));
  EEPROM.write(ADR_PULSE_WIDTH_TM+1, (unsigned char)((pulseWidthTM_ms>>0)&0xff));
}

/// Set the percent of max power of TM1
void setPercentMaxPowerTM1Function(void){

  Serial.println();
  Serial.print("Percent max power of TM1 (1-100%): ");

  // Enter with the value
  enterString();
  c = (unsigned char)atoi(myRxData);
  Serial.println();

  Serial.print("Value: ");
  Serial.print(c, DEC);
  Serial.println();

  if (c > 100){
    Serial.println("Out of range");
    return;
  }

  Serial.print("y/n: ");

  // check for keypress
  yn = enterChar();
  if(yn != 'y'){
    Serial.println("Abort");
    return;
  }

  percentMaxPowerTM1 = c;

  // writing in the eeprom
  EEPROM.write(ADR_PERCENT_MAX_POWER_TM1, percentMaxPowerTM1);

  Serial.println();
}

/// Set the percent of max power of TM2
void setPercentMaxPowerTM2Function(void){

  Serial.println();
  Serial.print("Percent max power of TM2 (1-100%): ");

  // Enter with the value
  enterString();
  c = (unsigned char)atoi(myRxData);
  Serial.println();

  if (c > 100){
    Serial.println("Out of range");
    return;
  }

  Serial.print("Value: ");
  Serial.print(c, DEC);
  Serial.println();

  Serial.print("y/n: ");

  // check for keypress
  yn = enterChar();
  if(yn != 'y'){
    Serial.println("Abort");
    return;
  }

  percentMaxPowerTM2 = c;

  // writing in the eeprom
  EEPROM.write(ADR_PERCENT_MAX_POWER_TM2, percentMaxPowerTM2);

  Serial.println();
}

/// Set the inverval between TM in continous mode
void setIntervalBetweenContinousTMFunction(void){

  Serial.println();
  Serial.print("Inverval between TM (0-65535ms): ");

  // Enter with the value
  enterString();
  i = (unsigned int)atoi(myRxData);
  Serial.println();

  Serial.print("Value: ");
  Serial.print(i, DEC);
  Serial.println("ms");

  Serial.print("y/n: ");

  // check for keypress
  yn = enterChar();
  if(yn != 'y'){
    Serial.println("Abort");
    return;
  }

  intervalBetweenContinuousTM_ms = i;

  // wrinting in the eeprom
  EEPROM.write(ADR_INTERVAL_BETWEEN_CONTINUOUS_TM_MS+0, (unsigned char)((intervalBetweenContinuousTM_ms>>8)&0xff));
  EEPROM.write(ADR_INTERVAL_BETWEEN_CONTINUOUS_TM_MS+1, (unsigned char)((intervalBetweenContinuousTM_ms>>0)&0xff));

  Serial.println();
}

/// Set the number of TM
void setNumberOfTMFunction(void){

  Serial.println();
  Serial.print("Number of TM (0=infinite or 1-65535): ");

  // Enter with the value
  enterString();
  i = (unsigned int)atoi(myRxData);
  Serial.println();

  Serial.print("Value: ");
  Serial.print(i, DEC);
  Serial.println();

  Serial.print("y/n: ");

  // check for keypress
  yn = enterChar();
  if(yn != 'y'){
    Serial.println("Abort");
    return;
  }

  numberOfTM = i;

  // wrinting in the eeprom
  EEPROM.write(ADR_NUMBER_OF_TM+0, (unsigned char)((numberOfTM>>8)&0xff));
  EEPROM.write(ADR_NUMBER_OF_TM+1, (unsigned char)((numberOfTM>>0)&0xff));

  Serial.println();
}

/// Set the initial delay before TM
void setInitialDelayTMFunction(void){

  Serial.println();
  Serial.print("Initial delay (0-65535ms): ");

  // Enter with the value
  enterString();
  i = (unsigned int)atoi(myRxData);
  Serial.println();

  Serial.print("Value: ");
  Serial.print(i, DEC);
  Serial.println("ms");

  Serial.print("y/n: ");

  // check for keypress
  yn = enterChar();
  if(yn != 'y'){
    Serial.println("Abort");
    return;
  }

  initialDelayTM_ms = i;

  // wrinting in the eeprom
  EEPROM.write(ADR_INITIAL_DELAY_TM_MS+0, (unsigned char)((initialDelayTM_ms>>8)&0xff));
  EEPROM.write(ADR_INITIAL_DELAY_TM_MS+1, (unsigned char)((initialDelayTM_ms>>0)&0xff));

  Serial.println();
}

/// Set the trigger mode: 1 for software and 0 for hardware
void setTriggerModeFunction(void){

  Serial.println();
  Serial.print("Trigger mode (s=software, h=hardware): ");

  // Enter with the value
  //enterString();
  //c = (unsigned char)atoi(myRxData);
  c = enterChar();
  Serial.println();

  Serial.print("Value: ");
  Serial.println(c);
  Serial.println();

  if ((c == 's') || (c == 'h')){
    // do nothing
  }
  else{
    Serial.println("Wrong option");
    return;
  }

  Serial.print("y/n: ");

  // check for keypress
  yn = enterChar();
  if(yn != 'y'){
    Serial.println("Abort");
    return;
  }

  triggerMode = c;

  // writing in the eeprom
  EEPROM.write(ADR_TRIGGER_MODE, triggerMode);

  Serial.println();
}

/// Print the parameters value
void getParametersFunction(void){

  Serial.println();

  i  = (unsigned int)(EEPROM.read(ADR_PULSE_WIDTH_TM+0)<<8);
  i |= (unsigned int)(EEPROM.read(ADR_PULSE_WIDTH_TM+1)<<0);
  Serial.print("TM width: ");
  Serial.print(i, DEC);
  Serial.println("ms");

  delay(50);

  c = (unsigned char)EEPROM.read(ADR_PERCENT_MAX_POWER_TM1);
  Serial.print("Power max of TM1: ");
  Serial.print(c, DEC);
  Serial.println("\%");
  //Serial.print("Duty cycle: "); 
  //Serial.print(CALC_DUTY_CYCLE(c), DEC);
  //Serial.println();

  delay(50);

  c = (unsigned char)EEPROM.read(ADR_PERCENT_MAX_POWER_TM2);
  Serial.print("Power max of TM2: ");
  Serial.print(c, DEC);
  Serial.println("\%");
  //Serial.print("Duty cycle: ");
  //Serial.print(CALC_DUTY_CYCLE(c), DEC);
  //Serial.println();

  delay(50);

  i  = (unsigned int)(EEPROM.read(ADR_INTERVAL_BETWEEN_CONTINUOUS_TM_MS+0)<<8);
  i |= (unsigned int)(EEPROM.read(ADR_INTERVAL_BETWEEN_CONTINUOUS_TM_MS+1)<<0);
  Serial.print("Interval between TM: ");
  Serial.print(i, DEC);
  Serial.println("ms");

  delay(50);

  i  = (unsigned int)(EEPROM.read(ADR_NUMBER_OF_TM+0)<<8);
  i |= (unsigned int)(EEPROM.read(ADR_NUMBER_OF_TM+1)<<0);
  Serial.print("Number of TM: ");
  Serial.print(i, DEC);
  Serial.println();

  delay(50);

  i  = (unsigned int)(EEPROM.read(ADR_INITIAL_DELAY_TM_MS+0)<<8);
  i |= (unsigned int)(EEPROM.read(ADR_INITIAL_DELAY_TM_MS+1)<<0);
  Serial.print("Initial delay: ");
  Serial.print(i, DEC);
  Serial.println("ms");

  delay(50);

  c = (unsigned char)EEPROM.read(ADR_TRIGGER_MODE);
  Serial.print("Trigger mode: ");
  Serial.print(c);
  Serial.println();
}

/// Check wich trigger signal was intercepted
// Return: s to starts, r to reset, and q to quit (software mode); 1 to start or 0 to quit (hardware mode)
unsigned char checkTriggerFunction(void){

  char status;

  if(triggerMode == 's'){
    if(Serial.available() != 0){ // Was a character sent by serial or the TM_START buttom was pressed?
      // a character was sent by serial!
      status = Serial.read(); // read the character
      if((status != 's') || (status != 'r') || (status == 'q')){
        //Serial.println("command not found");
      } 
    }  
  }

  if(triggerMode == 'h'){
    if(digitalRead(TM_TRIGGER_PIN) == HIGH){
      status = '1';
    }
    else{
      status = '0';
    }  
  }

  return status;
}

/// TM loop
void runLoopTMFunction(void){

  counterTM = 0;
  unsigned long time0;

  Serial.println();

  if((numberOfTM == 0) && (intervalBetweenContinuousTM_ms == 0)){
    //Serial.println("No TM and no intervals between TM");
    //getParametersFunction();
    return;
  }

  if(triggerMode == 'h'){
    // unreset the trigger
    digitalWrite(TM_TRIGGER_RESET_PIN, HIGH);
  }

  // infinite TM
  if((numberOfTM == 0) || (intervalBetweenContinuousTM_ms != 0)){
    // press start buttom to apply the thermal marks
    while(1){
      c = checkTriggerFunction();
      if(triggerMode == 's'){
        if(c == 's'){
          break;
        }
        if(c == 'q'){
          return;
        } 
      }
      if(triggerMode == 'h'){
        if(c == '1'){
          break;
        }
        if(c == '0'){
          //return;
        } 
      }
    }

    time0 = millis();

    // initial delay
    time0 = millis();
    while(((unsigned int)(millis()-time0)) < initialDelayTM_ms){
      c = checkTriggerFunction();
      if(triggerMode == 's'){
        if(c == 's'){
          //break;
        }
        if(c == 'q'){
          return;
        }
      }
      if(triggerMode == 'h'){
        if(c == '1'){
          //break;
        }
        if(c == '0'){
          return;
        } 
      }
    }
  }

  while((counterTM < numberOfTM) || (numberOfTM == 0)){

    // finite TM
    if ((numberOfTM != 0) && (intervalBetweenContinuousTM_ms == 0)){
      // press start buttom to apply the thermal marks
      while(1){
        c = checkTriggerFunction();
        if(triggerMode == 's'){
          if(c == 's'){
            break;
          }
          if(c == 'q'){
            return;
          }
        }
        if(triggerMode == 'h'){
          if(c == '1'){
            break;
          }
          if(c == '0'){
            //return;
          } 
        }
      }

      time0 = millis();

      // initial delay
      time0 = millis();
      while(((unsigned int)(millis()-time0)) < initialDelayTM_ms){
        c = checkTriggerFunction();
        if(triggerMode == 's'){
          if(c == 's'){
            //break;
          }
          if(c == 'q'){
            return;
          }
        }
        if(triggerMode == 'h'){
          if(c == '1'){
            //break;
          }
          if(c == '0'){
            return;
          } 
        }
      }
    }

    // turn the LED on
    digitalWrite(TM_LED_PIN, HIGH);

    // turn PWM A and PWM B on
    // - this signal will come out on the OC1 and OC1B I/O pin
    analogWrite(PWM_1_PIN, CALC_DUTY_CYCLE(percentMaxPowerTM1));
    analogWrite(PWM_2_PIN, CALC_DUTY_CYCLE(percentMaxPowerTM2));

    // apply pulse width
    time0 = millis();
    while(((unsigned int)(millis()-time0)) < pulseWidthTM_ms);

    // now turn all PWM off
    analogWrite(PWM_1_PIN, 0);
    analogWrite(PWM_2_PIN, 0);

    // turn the LED off
    digitalWrite(TM_LED_PIN, LOW);


    // infinte TM with interval between TM and controlled by buttons or start/reset character
    if((numberOfTM == 0) && (intervalBetweenContinuousTM_ms != 0)){
      // wait for the interval between TM
      time0 = millis();
      while(((unsigned int)(millis()-time0)) < intervalBetweenContinuousTM_ms){
        c = checkTriggerFunction();
        if(triggerMode == 's'){
          if(c == 'q'){
            return;
          }
        }
        if(triggerMode == 'h'){
          if(c == '0'){
            return;
          } 
        }
      }
    }

    // finite TM controlled by number of TM. Press reset buttom or send q character to abort loop
    if((numberOfTM != 0) && (intervalBetweenContinuousTM_ms != 0)){

      ++counterTM;

      //no need to reset in the last loop
      if(counterTM == numberOfTM){
        break;
      }

      time0 = millis();
      while(((unsigned int)(millis()-time0)) < intervalBetweenContinuousTM_ms){
        c = checkTriggerFunction();
        if(triggerMode == 's'){
          if(c == 'q'){
            return;
          }
        }
        if(triggerMode == 'h'){
          if(c == '0'){
            return;
          } 
        }
      }
    }

    // finite TM controlled by buttons or start/reset character
    if((numberOfTM != 0) && (intervalBetweenContinuousTM_ms == 0)){

      ++counterTM;

      // no need to reset in the last loop
      if(counterTM == numberOfTM){
        break;
      }

      // press reset buttom to prepare the thermal markers
      while(1){
        c = checkTriggerFunction();
        if(triggerMode == 's'){
          if(c == 'r'){
            break;
          }
          if(c == 'q'){
            return;
          }
        }
        if(triggerMode == 'h'){
          if(c == '0'){
            return;
          } 
        }
      }
    }

    // Get TM1 and TM2 status
    getTMStatusFunction();

  }
}

/// Test prints if some TM is burned.
// Resturn: 0b00 for non TM burned, 0b01 for TM1 burned, 0b10 for TM2 burned, and 0b11 for TM1 and TM2 burned.
unsigned char getTMStatusFunction(){

  unsigned char status = 0;

  //Serial.println();

  // TM1 status  
  if((digitalRead(PWM_1_PIN) == LOW) && (digitalRead(TM_1_STATUS_PIN) == HIGH)){
    //Serial.println("TM1 OK");
    status |= (0<<0);
  }
  else{
    Serial.println("TM1 opened");
    status |= (1<<0);
  }

  // TM2 status
  if((digitalRead(PWM_2_PIN) == LOW) && (digitalRead(TM_2_STATUS_PIN) == HIGH)){
    //Serial.println("TM2 OK");
    status |= (0<<1);
  }
  else{
    Serial.println("TM2 opened");
    status |= (1<<1);
  }

  return status;
}

